/*
 * Copyright 2018 The Cartographer Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */


#include "scan_matching/nearest_search.h"
#include "scan_matching/occupied_grid_cost_function_2d.h"

#include "mapping/probability_grid.h"
#include "mapping/probability_values.h"

namespace cartographer {
namespace scan_matching {

namespace {

// Computes a cost for matching the 'point_cloud' to the 'grid' with
// a 'pose'. The cost increases with poorer correspondence of the grid and the
// point observation (e.g. points falling into less occupied space).
class OccupiedGridCostFunction2D : public ceres::SizedCostFunction<1, 3> {
 public:
  OccupiedGridCostFunction2D(const double scaling_factor,
                              const sensor::PointCloud& point_cloud,
                              const transform::Rigid2d& initial_pose_estimate,
                              const Grid2D& grid) 
      : scaling_factor_(scaling_factor),
        point_cloud_(point_cloud),
        grid_(grid) {

      nearest_search_.BuildMapCSpaceByMaxProbability(point_cloud_, initial_pose_estimate, grid_);
    }

    virtual ~OccupiedGridCostFunction2D() {}
   
    virtual bool Evaluate(double const* const* parameters,
                          double* residual,
                          double** jacobians) const {
        // 解析参数
        Eigen::Matrix<double, 2, 1> translation(parameters[0][0], parameters[0][1]);
        Eigen::Rotation2D rotation(parameters[0][2]);
        Eigen::Matrix<double, 2, 2> rotation_matrix = rotation.toRotationMatrix();
        Eigen::Matrix<double, 3, 3> transform;
        transform << rotation_matrix, translation, 0., 0., 1.;
        
        // LOG(INFO) << " transform ===== : " << transform << std::endl;

        const MapLimits& limits = grid_.limits();

        for (size_t i = 0; i < point_cloud_.size(); ++i) {

          // LOG(INFO) << " idx ===== : " << i << ", " << point_cloud_.size() << std::endl;

          // Note that this is a 2D point. The third component is a scaling factor.
          const Eigen::Matrix<double, 3, 1> point(((point_cloud_[i].position.x())),
                                            ((point_cloud_[i].position.y())),
                                            (1.));
          // 根据预测位姿对单个点进行坐标变换
          const Eigen::Matrix<double, 3, 1> world = transform * point;
          Eigen::Vector2d world_pose = world.head<2>();
          Eigen::Array2d cell_double_idx = limits.GetCellDoubleIndex(world_pose); 

          // Bilinear Interpolation
          Eigen::Array2i cell_idx00 = limits.GetCellIndex(Eigen::Vector2f(world[0], world[1])); 
          Eigen::Array2i cell_idx10(cell_idx00[0] + 1, cell_idx00[1]);
          Eigen::Array2i cell_idx01(cell_idx00[0], cell_idx00[1] + 1);
          Eigen::Array2i cell_idx11(cell_idx00[0] + 1, cell_idx00[1] + 1);

          // LOG(INFO) << " cell_idx00 ===== : " << cell_idx00.transpose() << std::endl;
          // LOG(INFO) << " cell_idx10 ===== : " << cell_idx10.transpose() << std::endl;
          // LOG(INFO) << " cell_idx01 ===== : " << cell_idx01.transpose() << std::endl;
          // LOG(INFO) << " cell_idx11 ===== : " << cell_idx11.transpose() << std::endl;

          if(!limits.Contains(cell_idx00) || !limits.Contains(cell_idx10) ||
             !limits.Contains(cell_idx01) ||
             !limits.Contains(cell_idx11)) {
             double scaling_factor = scaling_factor_ * 1e-10;

             residual[i] = scaling_factor * (1.0 - /*grid_.GetMinCorrespondenceCost()*/0.0);

             // LOG(INFO) << " residual[i] ===== : " << residual[i] << std::endl;

             if(jacobians != nullptr && jacobians[0] != nullptr) {
                Eigen::Map<Eigen::Matrix<double, 1, 3, Eigen::RowMajor>> jacobian(jacobians[0]);
                // jacobian.setOnes();
                // jacobian *= 1e10;

                jacobian.setZero();
                
                // LOG(INFO) << " jacobian ===== : " << jacobian.transpose() << std::endl;
             }
            
             // LOG(INFO) << " jacobians[0] ===== : " << jacobians[0][0] << " , " << jacobians[0][1] << " , " << jacobians[0][2] << std::endl;
             // LOG(INFO) << " scaling_factor ===== : " << scaling_factor << std::endl;

             continue;
          }

          // double p00 = static_cast<const ProbabilityGrid&>(grid_).GetProbability(cell_idx00);
          // double p10 = static_cast<const ProbabilityGrid&>(grid_).GetProbability(cell_idx10);
          // double p01 = static_cast<const ProbabilityGrid&>(grid_).GetProbability(cell_idx01);
          // double p11 = static_cast<const ProbabilityGrid&>(grid_).GetProbability(cell_idx11);

          // LOG(INFO) << " cell_double_idx ===== : " << cell_double_idx.transpose() << std::endl;

          float p00 = nearest_search_.ComputeProbBilinearInterpolate(cell_idx00);
          float p10 = nearest_search_.ComputeProbBilinearInterpolate(cell_idx10);
          float p01 = nearest_search_.ComputeProbBilinearInterpolate(cell_idx01);
          float p11 = nearest_search_.ComputeProbBilinearInterpolate(cell_idx11);

          // LOG(INFO) << " p00 ===== : " << p00 << std::endl;
          // LOG(INFO) << " p10 ===== : " << p10 << std::endl;
          // LOG(INFO) << " p01 ===== : " << p01 << std::endl;
          // LOG(INFO) << " p11 ===== : " << p11 << std::endl;

          double x = cell_double_idx[0];
          double y = cell_double_idx[1];

          double x0 = cell_idx00[0];
          double y0 = cell_idx00[1];
          double x1 = cell_idx11[0];
          double y1 = cell_idx11[1];    

          // LOG(INFO) << " y1-y0 ===== : " << y1-y0 << std::endl;
          // LOG(INFO) << " x1-x0 ===== : " << x1-x0<< std::endl;

          double probability = (y-y0)/(y1-y0) * ((x-x0)/(x1-x0)*p11+(x1-x)/(x1-x0)*p01) +
                               (y1-y)/(y1-y0) * ((x-x0)/(x1-x0)*p10+(x1-x)/(x1-x0)*p00);

          // LOG(INFO) << " probability ===== : " << probability << std::endl;

          residual[i] = 1.0 - probability;

          // free值越小, 表示占用的概率越大
          residual[i] = scaling_factor_ * residual[i];

          // LOG(INFO) << " scaling_factor_ ===== : " << scaling_factor_ << std::endl;

          if(jacobians != nullptr && jacobians[0] != nullptr) {
            double dlx = (y-y0)/(y1-y0) * (p11-p01) + (y1-y)/(y1-y0) * (p10-p00);
            double dly = (x-x0)/(x1-x0) * (p11-p10) + (x1-x)/(x1-x0) * (p01-p00);
            Eigen::Matrix<double, 1, 2> jac_map;
            jac_map << dlx, dly;

            Eigen::Matrix<double, 2, 3> jac_pose;
            jac_pose.block<2, 2>(0, 0) = Eigen::Matrix<double, 2, 2>::Identity();
            float px = point[0];
            float py = point[1];
            jac_pose(0, 2) = px * rotation_matrix(0, 1) - py * rotation_matrix(0, 0); 
            jac_pose(1, 2) = px * rotation_matrix(1, 1) - py * rotation_matrix(1, 0);

            Eigen::Map<Eigen::Matrix<double, 1, 3, Eigen::RowMajor>> jacobian(jacobians[0]);
            jacobian = jac_map * jac_pose;

            // LOG(INFO) << " y-y0 ===== : " << y-y0 << std::endl;
            // LOG(INFO) << " y1-y ===== : " << y1-y << std::endl;
            // LOG(INFO) << " x-x0 ===== : " << x-x0 << std::endl;
            // LOG(INFO) << " x1-x ===== : " << x1-x << std::endl;

            // LOG(INFO) << " jac_map ===== : " << jac_map << std::endl;
            // LOG(INFO) << " jac_pose ===== : " << jac_pose << std::endl;

            // LOG(INFO) << " jacobian ===== : " << jacobian.transpose() << std::endl;

            // LOG(INFO) << " jacobians[0] ===== : " << jacobians[0][0] << " , " << jacobians[0][1] << " , " << jacobians[0][2] << std::endl;
          }
        }

        // LOG(INFO) << " transform ===== : " << transform << std::endl;

        return true;
    }

 private:
  OccupiedGridCostFunction2D(const OccupiedGridCostFunction2D&) = delete;
  OccupiedGridCostFunction2D& operator=(const OccupiedGridCostFunction2D&) =
      delete;

  const double scaling_factor_;
  const sensor::PointCloud& point_cloud_;
  const Grid2D& grid_;

  NearestSearch nearest_search_;
};

}  // namespace

// 工厂函数, 返回地图的CostFunction
ceres::CostFunction* CreateOccupiedGridCostFunction2D(
    const double scaling_factor, const sensor::PointCloud& point_cloud, const transform::Rigid2d& initial_pose_estimate,
    const Grid2D& grid) {
  return new OccupiedGridCostFunction2D(scaling_factor, point_cloud, initial_pose_estimate, grid); // 比固定残差维度的 多了一个参数
}

}  // namespace scan_matching
}  // namespace cartographer
